#include <FreeRTOS_AVR.h>
const short servo[5] = {};
const short switch1, switch2, switch3, switch4;
const short relay;

SemaphoreHandle_t sem_forward;
SemaphoreHandle_t sem_backward;
SemaphoreHandle_t sem_dip_angle_increase;
SemaphoreHandle_t sem_dip_angle_decrease;

#define s_delay(A) vTaskDelay((AL * configTICK_RATE_HZ) / 1000L)

void forward()
{
}

void backward() {}

void dip_angle_increase() {}

void dip_angle_decrease() {}

void initio() {}

static void check_switch(void *arg)
{
  pinMode(switch1, INPUT_PULLUP);
  pinMode(switch2, INPUT_PULLUP);
  pinMode(switch3, INPUT_PULLUP);
  pinMode(switch4, INPUT_PULLUP);
  while (1)
  {
    if (digitalRead(switch1) == LOW)
    {
      xSemaphoreGive(sem_forward);
      vTaskDelay((200L * configTICK_RATE_HZ) / 1000L);
    }
    if (digitalRead(switch2) == LOW)
    {
      xSemaphoreGive(sem_backward);
      vTaskDelay((200L * configTICK_RATE_HZ) / 1000L);
    }
    if (digitalRead(switch3) == LOW)
    {
      xSemaphoreGive(sem_dip_angle_increase);
      vTaskDelay((200L * configTICK_RATE_HZ) / 1000L);
    }
    if (digitalRead(switch4) == LOW)
    {
      xSemaphoreGive(sem_dip_angle_decrease);
      vTaskDelay((200L * configTICK_RATE_HZ) / 1000L);
    }
  }
}

static void forward_task(void *arg)
{
  while (1)
  {
    xSemaphoreTake(sem_forward, portMAX_DELAY);
    forward();
    while (digitalRead(switch1) == LOW)
      ;
  }
}

static void backward_task(void *arg)
{
  while (1)
  {
    xSemaphoreTake(sem_backward, portMAX_DELAY);
    backward();
    while (digitalRead(switch1) == LOW)
      ;
  }
}

static void dip_angle_increase_task(void *arg)
{
  while (1)
  {
    xSemaphoreTake(sem_dip_angle_increase, portMAX_DELAY);
    dip_angle_increase();
    while (digitalRead(switch3) == LOW)
      ;
  }
}

static void dip_angle_decrease_task(void *arg)
{
  while (1)
  {
    xSemaphoreTake(sem_dip_angle_decrease, portMAX_DELAY);
    dip_angle_decrease();
    while (digitalRead(switch4) == LOW)
      ;
  }
}

void error_fuck()
{
  pinMode(13, OUTPUT);
  digitalWrite(13, HIGH);
  while (1)
    ;
}

void setup()
{
  initio();
  portBASE_TYPE s[5];
  sem_backward = xSemaphoreCreateCounting(1, 0);
  sem_forward = xSemaphoreCreateCounting(1, 0);
  sem_dip_angle_increase = xSemaphoreCreateCounting(1, 0);
  sem_dip_angle_decrease = xSemaphoreCreateCounting(1, 0);
  s[0] =
      xTaskCreate(check_switch, NULL, configMINIMAL_STACK_SIZE, NULL, 1, NULL);
  s[1] =
      xTaskCreate(forward_task, NULL, configMINIMAL_STACK_SIZE, NULL, 2, NULL);
  s[2] =
      xTaskCreate(backward_task, NULL, configMINIMAL_STACK_SIZE, NULL, 3, NULL);
  s[3] = xTaskCreate(dip_angle_increase_task, NULL, configMINIMAL_STACK_SIZE,
                     NULL, 4, NULL);
  s[4] = xTaskCreate(dip_angle_decrease_task, NULL, configMINIMAL_STACK_SIZE,
                     NULL, 5, NULL);
  for (int i = 0; i < 5; i++)
    if (s[i] != pdPASS)
      error_fuck();
  if (sem_backward == NULL | sem_forward == NULL |
      sem_dip_angle_increase == NULL | sem_dip_angle_decrease == NULL)
    error_fuck();
  vTaskStartScheduler();
  while (1)
    ; // Don't edit!
}

void loop()
{
  // Do nothing
}
